/**
 * @brief SensAct.h
 */

#ifndef SENSACT_H_
#define SENSACT_H_

#include "Config.h"
#include "Types.h"

const double const_ZERO = 0.0;

class Sensors
{
public:
  Sensors()
  {

  }
  virtual ~Sensors(){}

  double& operator[](const int& index)
  {
      return sensors[index];
  }

  const double& operator[](const int& index) const
  {
      return sensors[index];
  }

private:
  double sensors[NUM_SENS];
};

class Motor
{
public:
  Motor()
  : speed_left(0.0)
  , speed_right(0.0)
  {

  }
  virtual ~Motor(){}

  void stop()
  {
    speed_left = 0.0;
    speed_right = 0.0;
  }

  void move(const double& speedLeft, const double& speedRight)
  {
    speed_left = speedLeft;
    speed_right = speedRight;
  }

  void toggleLR()
  {
    speed_left = speed_right;
    speed_right = speed_left;
  }
  void toggleFB()
  {
    speed_left = -speed_left;
    speed_right = -speed_right;
  }

  int getSpeedRight()
  {
    return speed_right;
  }
  int getSpeedLeft()
  {
    return speed_left;
  }

  void convertToMaxSpeed()
  {
//    if(speed_left <= 1.0 && speed_left >= -1.0)
      speed_left *= MAX_SPEED;
//    if(speed_right <= 1.0 && speed_right >= -1.0)
      speed_right *= MAX_SPEED;
  }

private:
  double speed_left;
  double speed_right;
};

#endif /* SENSACT_H_ */
